#include <dispatch_system_ros_viz/vehicle_viz.h>

namespace moying
{
namespace visualization
{
    VehicleViz::VehicleViz(const VehiclePtr &vehicle, const MoyingMapPtr &moyingmap, const std::string &global_frame)
    : nh_("MoyingVehicleViz"), vehicle_(vehicle), moyingmap_(moyingmap), global_frame_(global_frame)
    {
        traj_pub_ = nh_.advertise<geometry_msgs::PoseArray>("r" + std::to_string(vehicle_->getID()) + "/trajectory",10, true);
        collide_rect_pub_ = nh_.advertise<geometry_msgs::PolygonStamped>("r" + std::to_string(vehicle_->getID()) + "/collide",10, true);
        traj_array_pub_ = nh_.advertise<visualization_msgs::MarkerArray>("r" + std::to_string(vehicle_->getID()) + "/trajectory_array",10, true);
        time_footprint_pub_ = nh_.advertise<visualization_msgs::MarkerArray>("r" + std::to_string(vehicle_->getID()) + "/astar_footprint",10, true);
        traj_cells_pub_ = nh_.advertise<visualization_msgs::Marker>("r" + std::to_string(vehicle_->getID()) + "/cell_trajectory",10, true);
        pose_pub_ = nh_.advertise<visualization_msgs::Marker>("r" + std::to_string(vehicle_->getID()) + "/TargetPose",10, true);
        robot_pose_pub_ = nh_.advertise<visualization_msgs::MarkerArray>("r" + std::to_string(vehicle_->getID()) + "/occupency",10, true);
        // robot_pose_pub_ = nh_.advertise<visualization_msgs::Marker>("r" + std::to_string(vehicle_->getID()) + "/occupency",10, true);
        obs_occupancy_pub_ = nh_.advertise<visualization_msgs::MarkerArray>("r" + std::to_string(vehicle_->getID()) + "/obstacle_occupency",10, true);
        close_nodes_pub_ = nh_.advertise<visualization_msgs::Marker>("r" + std::to_string(vehicle_->getID()) + "/astar_search_nodes",10, true);
        // local_costmap_pub_ = nh_.advertise<visualization_msgs::Marker>("r" + "/LocalCostmap",10, true);
        local_obstacle_pub_ = nh_.advertise<visualization_msgs::MarkerArray>("r" +std::to_string(vehicle_->getID()) + "_" + "/LocalObstacle",10, true);
        local_temp_obstacle_pub_ = nh_.advertise<visualization_msgs::MarkerArray>("r" +std::to_string(vehicle_->getID()) + "_" + "/LocalTempObstacle",10, true);
        local_cost_map_pub_ = nh_.advertise<visualization_msgs::MarkerArray>("r" +std::to_string(vehicle_->getID()) + "_" + "/LocalCostMap",10, true); 

    }

    VehicleViz::~VehicleViz()
    {

    }

    void VehicleViz::publishCellTrajectory(const std::map<int, std::vector<FootprintCell>> &timed_footprints)
    {
        CostMapPtr costmap = moyingmap_->getCostMap();
        double resolution = costmap->getResolution();
        int sx = costmap->getSizeInCellsX();
        int sy = costmap->getSizeInCellsY();

        int st = moyingmap_->getTimeCounts();
        visualization_msgs::Marker marker;
        marker.header.frame_id = global_frame_;
        marker.header.stamp = ros::Time::now();
        marker.id = 0;
        marker.action = visualization_msgs::Marker::ADD;
        marker.pose.orientation.w = 1.0;
        marker.scale.x = 0.9 * resolution;
        marker.scale.y = 0.9 * resolution;
        marker.scale.z = 0.1;
        marker.color.a = 1;
        marker.type = visualization_msgs::Marker::CUBE_LIST;
        
        for(auto &it: timed_footprints)
        {
            for(int i = 0; i < it.second.size(); i++)
            {
                double wx, wy;
                costmap->mapToWorld(it.second[i].x, it.second[i].y, wx, wy);
                geometry_msgs::Point p;
                p.x = wx;
                p.y = wy;
                p.z = 0.1 * it.first;
                // marker.points.push_back(p);
                std_msgs::ColorRGBA c;
                
                if(it.second[i].inflate)
                {
                    c.r = c.g = c.b = 0.9;
                    c.a = 0.2;
                }
                else 
                {
                    c = getColorMsg();
                    marker.points.push_back(p);
                    marker.colors.push_back(c);
                }
            }
        }

        traj_cells_pub_.publish(marker);
    }

    geometry_msgs::Pose VehicleViz::getPoseMsg(const VehicleState &state)
    {
        return getPoseMsg(state.x, state.y, state.th);
    }

    geometry_msgs::Pose VehicleViz::getPoseMsg(double x, double y, double th)
    {
        geometry_msgs::Pose p;
        p.position.x = x;
        p.position.y = y;
        p.position.z = 0;
        double yaw = th;
        double pitch = 0, roll = 0;
        double cy = cos(yaw * 0.5);
        double sy = sin(yaw * 0.5);
        double cp = cos(pitch * 0.5);
        double sp = sin(pitch * 0.5);
        double cr = cos(roll * 0.5);
        double sr = sin(roll * 0.5);
        p.orientation.w = cy * cp * cr + sy * sp * sr;
        p.orientation.x = cy * cp * sr - sy * sp * cr;
        p.orientation.y = sy * cp * sr + cy * sp * cr;
        p.orientation.z = sy * cp * cr - cy * sp * sr;
        return p;
    }

    void VehicleViz::publishTrajectory(const PlanningResult &result)
    {
        geometry_msgs::PoseArray poses;
        poses.header.frame_id = global_frame_;
        poses.header.stamp = ros::Time::now();
        double z = 0;
        for(int i = 0; i < result.path.size(); i++)
        {
            geometry_msgs::Pose p = getPoseMsg(result.path[i]);
            p.position.z = z;
            z += 0.02;
            poses.poses.push_back(p);
        }

        traj_pub_.publish(poses);
        
        CostMapPtr costmap = moyingmap_->getCostMap();
        visualization_msgs::MarkerArray marker_array;
        marker_array.markers.clear();
        for(int i = 0; i < result.node_path.size(); i++)
        {
            visualization_msgs::Marker marker;
            marker.header.frame_id = global_frame_;
            marker.header.stamp = ros::Time::now();
            marker.id = i;
            marker.action = visualization_msgs::Marker::ADD;
            costmap->mapToWorld(result.node_path[i].x, result.node_path[i].y, marker.pose.position.x, marker.pose.position.y);
            marker.pose.orientation.w = 1;
            marker.scale.z = 0.2;
            marker.color.g = 1;
            marker.color.a = 1;
            marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
            marker.text = "(" + std::to_string(result.node_path[i].x) + ", " + std::to_string(result.node_path[i].y) + ")";
            marker_array.markers.push_back(marker);
        }
        traj_array_pub_.publish(marker_array);
    }

    
    void VehicleViz::publishTimeFootprint(const PlanningResult &result)
    {
        
        CostMapPtr costmap = moyingmap_->getCostMap();
        visualization_msgs::MarkerArray marker_array;
        marker_array.markers.clear();
        int i=0;
        for(auto item : result.timed_footprints)
        {
            for(auto cell : item.second){
                i++;
                visualization_msgs::Marker marker;
                marker.header.frame_id = global_frame_;
                marker.header.stamp = ros::Time::now();
                marker.id = i;
                marker.action = visualization_msgs::Marker::ADD;
                costmap->mapToWorld(cell.x, cell.y, marker.pose.position.x, marker.pose.position.y);
                marker.pose.position.z = item.first*0.2;
                marker.pose.orientation.w = 1;
                marker.scale.x = 0.4;
                marker.scale.y = 0.4;
                marker.scale.z = 0.05;
                if(cell.inflate){
                    marker.color.r = marker.color.g = marker.color.b = 0;
                    marker.color.a = 0.1; 
                }else{
                    marker.color.g = 1;
                    marker.color.a = 0.3;
                }
                marker.type = visualization_msgs::Marker::CUBE;
                marker_array.markers.push_back(marker);
            }
            
        }
        time_footprint_pub_.publish(marker_array);
    }

    void VehicleViz::publishCurrentPose()
    {
        navigation::VehicleState state = vehicle_->getPose();
        // publishPose
    }

    void VehicleViz::publishPose(double x, double y, double theta)
    {
        visualization_msgs::Marker marker;
        marker.header.frame_id = global_frame_;
        marker.header.stamp = ros::Time::now();
        marker.id = 0;
        marker.action = visualization_msgs::Marker::ADD;
        marker.pose = getPoseMsg(x, y, theta);
        marker.scale.x = vehicle_->getLength() / 2.0;
        marker.scale.y = vehicle_->getWidth() / 2.0;
        marker.scale.z = 0.1;
        marker.color.a = 1;
        marker.color.r = 1;
        marker.type = visualization_msgs::Marker::CUBE;
        pose_pub_.publish(marker);
    }

    std_msgs::ColorRGBA VehicleViz::getColorMsg()
    {

        std_msgs::ColorRGBA msg;
        if(vehicle_ != nullptr){
            navigation::ColorRGBA color = vehicle_->getColor();
            msg.r = color.r;
            msg.g = color.g;
            msg.b = color.b;
            msg.a = color.a;
        }
        
        return msg;
    }

    void fromIndex(int index,int& x, int& y, int& th, int& t, int& sm,int minx,int miny,int mint)
    {
        int size_x_,size_y_,size_th_,size_sm_;
        size_x_ = 14;
        size_y_ = 14;
        size_sm_ = 2;
        size_th_ = 8;

        x = index % size_x_+minx;
        index /=  size_x_;
        y = index  % size_y_+miny;
        index /=  size_y_;
        th = index  % size_th_;
        index /=  size_th_;
        if(size_sm_ > 0){
            sm = index % size_sm_;
            index /= size_sm_;
            t = index+mint;
        }else{
            t = index+mint;
        }
        return ;
    }

    void VehicleViz::publishAStarSearchNodes(const boost::dynamic_bitset<>& nodes,bool closed,bool local_map,int minx,int miny,int mint)
    {
        visualization_msgs::Marker marker;
        {
            marker.header.frame_id = "map";
            marker.header.stamp = ros::Time::now();
            marker.id = 0;
            marker.action = visualization_msgs::Marker::ADD;
            marker.pose.orientation.w = 1;
            marker.scale.x = 0.4;
            marker.scale.y = 0.4;
            marker.scale.z = 0.05;
            marker.color.a = 1;
            marker.type = visualization_msgs::Marker::CUBE_LIST;
            int x, y,th,t;
            MapDimension d = moyingmap_->getDimensions();
            int size = d.x*d.y;
            boost::dynamic_bitset<> exist_bits;
            exist_bits.resize(size,false);

            int nodes_size = nodes.size() ;
            for(int i = 0; i < nodes_size; i++)
            {
                if(!nodes[i]){
                    continue;
                }

                // moyingmap_->fromIndex(i,x,y,th,t);
                int sm;
                if(local_map){
                    fromIndex(i,x,y,th,t,sm,minx,miny,mint);
                }else{
                    moyingmap_->fromIndex(i,x,y,th,t,sm);
                }
                // WARNING("x:"<<x<<" y:"<<y<<" th:"<<th<<" sm:"<<sm);
                // if(sm == 1)
                //     ERROR("sm:"<<sm<<" x:"<<x<<" y:"<<y);
                int n = x + y*d.x;
                if(n >= size){
                    // printf("wrong x:%d y:%d\n",x,y);
                    continue;
                }

                // if(t>60)
                //     continue;
                if(exist_bits[n])
                    continue;
                else
                {
                    exist_bits[n] = true;
                }

// if(sm == 1){
//     printf("-----------sm==1\n");
// }
                geometry_msgs::Point p;
                moyingmap_->getCostMap()->mapToWorld(x, y, p.x, p.y);
                p.z = t*0.1;
                // p.z = sm*2;
                std_msgs::ColorRGBA c;
                c.r = 1;
                c.g = 0;
                c.b = 0;
                c.a = 0.5;
                marker.points.push_back(p);
                marker.colors.push_back(c);
                
            }
        }

        close_nodes_pub_.publish(marker);
    }

    void VehicleViz::publishRobotOccupancy()
    {
        visualization_msgs::MarkerArray marker_array;
        marker_array.markers.clear();
        std::map<int, navigation::FootprintCell> footprint_cells;
        navigation::VehicleState state;
        vehicle_->getPose(state);
        vehicle_->getFootprintOccupiedCells(moyingmap_->getCostMap(),state,footprint_cells);

        // printf("pose x:%.4f y:%.4f\n",state.x,state.y);
        // for(auto item : footprint_cells){
        //     printf(" x:%d y:%d \n",item.second.x,item.second.y);
        // }
        int i=0;
        for(auto item : footprint_cells)
        {
            visualization_msgs::Marker marker;
            marker.header.frame_id = global_frame_;
            marker.header.stamp = ros::Time::now();
            marker.id = ++i;
            marker.action = visualization_msgs::Marker::ADD;
            marker.type = visualization_msgs::Marker::CUBE;

            geometry_msgs::Point p;
            moyingmap_->getCostMap()->mapToWorld(item.second.x, item.second.y, p.x, p.y);
            marker.pose.position.x = p.x;
            marker.pose.position.y = p.y;
            marker.pose.position.z = 0.05;
            marker.pose.orientation.w = 1;
            float width = moyingmap_->getCostMap()->getResolution()-0.02;
            marker.scale.x = width;
            marker.scale.y = width;
            marker.scale.z = 0.05;
            std_msgs::ColorRGBA c;
            if(item.second.inflate){
                    c.r = 0; c.g = 0; c.b = 1; c.a = 0.3;
            }else{
                c.r = 1; c.g = 0; c.b = 0; c.a = 0.5;
            }
            marker.color = c;
            marker_array.markers.push_back(marker);
        }

        robot_pose_pub_.publish(marker_array);
    }

    // void VehicleViz::publishRobotOccupancy()
    // {
    //     visualization_msgs::Marker marker;
    //     {
    //         marker.header.frame_id = "map";
    //         marker.header.stamp = ros::Time::now();
    //         marker.id = 0;
    //         marker.action = visualization_msgs::Marker::ADD;
    //         marker.pose.orientation.w = 1;
    //         marker.scale.x = 0.45;
    //         marker.scale.y = 0.45;
    //         marker.scale.z = 0.05;
    //         marker.color.a = 1;
    //         marker.type = visualization_msgs::Marker::CUBE_LIST;

    //         std::map<int, navigation::FootprintCell> footprint_cells;
    //         navigation::VehicleState state;
    //         vehicle_->getPose(state);
    //         vehicle_->getFootprintOccupiedCells(moyingmap_->getCostMap(),state,footprint_cells);

    //         for(auto item : footprint_cells)
    //         {

    //             geometry_msgs::Point p;
    //             moyingmap_->getCostMap()->mapToWorld(item.second.x, item.second.y, p.x, p.y);
    //             std_msgs::ColorRGBA c;
    //             if(item.second.inflate){
    //                 c.r = 0; c.g = 0; c.b = 1; c.a = 0.1;
    //             }else{
    //                  c.r = 0; c.g = 1; c.b = 0; c.a = 0.5;
    //             }
                
    //             marker.points.push_back(p);
    //             marker.colors.push_back(c);
                
    //         }
    //     }

    //     robot_pose_pub_.publish(marker);
    // }

    void VehicleViz::publishObstacleOccupancy(const std::vector<std::pair<int,int>>& obstacles)
    {

        visualization_msgs::MarkerArray marker_array;
        marker_array.markers.clear();

        int i=0;
        for(auto obs : obstacles)
        {
            visualization_msgs::Marker marker;
            marker.header.frame_id = global_frame_;
            marker.header.stamp = ros::Time::now();
            marker.id = ++i;
            marker.action = visualization_msgs::Marker::ADD;
            marker.type = visualization_msgs::Marker::CUBE;

            geometry_msgs::Point p;
            moyingmap_->getCostMap()->mapToWorld(obs.first, obs.second, p.x, p.y);
            marker.pose.position.x = p.x;
            marker.pose.position.y = p.y;
            marker.pose.position.z = 0.05;
            marker.pose.orientation.w = 1;
            float width = moyingmap_->getCostMap()->getResolution()-0.02;
            marker.scale.x = width;
            marker.scale.y = width;
            marker.scale.z = 0.05;
            std_msgs::ColorRGBA c;
            c.r = 1; c.g = 0; c.b = 0; c.a = 0.3;
            marker.color = c;
            marker_array.markers.push_back(marker);
        }
        obs_occupancy_pub_.publish(marker_array);


        // visualization_msgs::Marker marker;
        // {
        //     marker.header.frame_id = "map";
        //     marker.header.stamp = ros::Time::now();
        //     marker.id = 0;
        //     marker.action = visualization_msgs::Marker::ADD;
        //     marker.pose.orientation.w = 1;
        //     marker.scale.x = 0.45;
        //     marker.scale.y = 0.45;
        //     marker.scale.z = 0.05;
        //     marker.color.a = 1;
        //     marker.type = visualization_msgs::Marker::CUBE_LIST;

        //     for(auto obs : obstacles)
        //     {

        //         geometry_msgs::Point p;
        //         moyingmap_->getCostMap()->mapToWorld(obs.first, obs.second, p.x, p.y);
        //         std_msgs::ColorRGBA c;
        //         c.r = 1; c.g = 0; c.b = 0; c.a = 0.3;
                
                
        //         marker.points.push_back(p);
        //         marker.colors.push_back(c);
                
        //     }
        // }

        // obs_occupancy_pub_.publish(marker);
    }

    // void VehicleViz::publishLocalCostmap(const moying_proto::LocalCostMap& local_costmap)
    // {
    //     visualization_msgs::Marker marker;
    //     {

    //         int size = local_costmap.width() * local_costmap.height();
    //         if(size == 0 && local_costmap.data().size() != 0)
    //             return;
    //         marker.header.frame_id = "map";
    //         marker.header.stamp = ros::Time::now();
    //         marker.id = 0;
    //         marker.action = visualization_msgs::Marker::ADD;
    //         marker.pose.orientation.w = 1;
    //         marker.scale.x = 0.4;
    //         marker.scale.y = 0.4;
    //         marker.scale.z = 0.05;
    //         marker.color.a = 1;
    //         marker.type = visualization_msgs::Marker::CUBE_LIST;
    //         int x, y;
    //         for(int i = 0; i < local_costmap.data().size(); i++)
    //         {
    //             x = i % local_costmap.width() + local_costmap.origin_x();
    //             if(local_costmap.width() == 0){
    //                 ERROR("divisor is zero in publishVisualization!\n");
    //             }
    //             y = i / local_costmap.width() + local_costmap.origin_y();
    //             geometry_msgs::Point p;
    //             moyingmap_->getCostMap()->mapToWorld(x, y, p.x, p.y);
    //             std_msgs::ColorRGBA c;
    //             if(local_costmap.data()[i] == 0){
    //                 c.r = 0; c.g=0; c.b=0; c.a = 0.1;
    //             }else{
    //                 c.r = 1;
    //                 c.a = 0.3;
    //             }
                
    //             marker.points.push_back(p);
    //             marker.colors.push_back(c);
    //         }
    //     }
    //     local_costmap_pub_.publish(marker);
    // }


    void VehicleViz::publishLocalObstacle(const moying_proto::LocalObstacle& local_obstacle,bool temp)
    {

        visualization_msgs::MarkerArray marker_array;
        marker_array.markers.clear();

        int size = local_obstacle.width() * local_obstacle.height();
        if(size == 0 && local_obstacle.data().size() != 0)
            return;
            
            // printf("obs size:%d\n",local_obstacle.data().size());
        int i=0;
        for(auto obstacle_pos : local_obstacle.data())
        {
            visualization_msgs::Marker marker;
            marker.header.frame_id = global_frame_;
            marker.header.stamp = ros::Time::now();
            marker.id = ++i;
            marker.action = visualization_msgs::Marker::ADD;
            marker.type = visualization_msgs::Marker::CUBE;

            int x, y;
            geometry_msgs::Point p;
            x = obstacle_pos.x();
            y = obstacle_pos.y();
            p.x = local_obstacle.origin_x() + x * moyingmap_->getCostMap()->getRawResolution();
            p.y = local_obstacle.origin_y() + y * moyingmap_->getCostMap()->getRawResolution();
            marker.pose.position.x = p.x;
            marker.pose.position.y = p.y;
            marker.pose.position.z = 0.05;
            marker.pose.orientation.w = 1;
            marker.scale.x = 0.03;
            marker.scale.y = 0.03;
            marker.scale.z = 0.03;

            std_msgs::ColorRGBA c;
            if(temp){
                c.b = 1;
                c.a = 1;
            }else{
                c.r = 1;
                c.a = 1;
            }
            marker.color = c;
            marker_array.markers.push_back(marker);
        }
        if(!temp)
            local_obstacle_pub_.publish(marker_array);
        else
            local_temp_obstacle_pub_.publish(marker_array);


        // visualization_msgs::Marker marker;
        // {
        //     int size = local_obstacle.width() * local_obstacle.height();
        //     if(size == 0 && local_obstacle.data().size() != 0)
        //         return;
        //     marker.header.frame_id = "map";
        //     marker.header.stamp = ros::Time::now();
        //     marker.id = 0;
        //     marker.action = visualization_msgs::Marker::ADD;
        //     marker.pose.orientation.w = 1;
        //     marker.scale.x = 0.05;
        //     marker.scale.y = 0.05;
        //     marker.scale.z = 0.05;
        //     marker.color.a = 1;
        //     marker.type = visualization_msgs::Marker::CUBE_LIST;
        //     int x, y;
        //     // for(int i = 0; i < local_obstacle.data().size(); i++)
        //     for(auto obstacle_pos : local_obstacle.data())
        //     {
        //         geometry_msgs::Point p;
        //         x = obstacle_pos.x();
        //         y = obstacle_pos.y();
        //         p.x = local_obstacle.origin_x() + x * moyingmap_->getCostMap()->getRawResolution();
        //         p.y = local_obstacle.origin_y() + y * moyingmap_->getCostMap()->getRawResolution();
                
        //         // moyingmap_->getCostMap()->mapToWorld(x, y, p.x, p.y);
        //         std_msgs::ColorRGBA c;
        //         if(temp){
        //             c.b = 1;
        //             c.a = 1;
        //         }else{
        //             c.r = 1;
        //             c.a = 1;
        //         }
        //         marker.points.push_back(p);
        //         marker.colors.push_back(c);
        //     }
        // }
        // if(!temp)
        //     local_obstacle_pub_.publish(marker);
        // else
        //     local_temp_obstacle_pub_.publish(marker);
    }

    void VehicleViz::publishLocalCostMap(std::vector<std::vector<unsigned char>> local_cost_map,int min_cx,int min_cy)
    {

        visualization_msgs::MarkerArray marker_array;
        marker_array.markers.clear();

        int i=0;
        for(int x=0;x<local_cost_map.size();x++){
            for(int y=0;y<local_cost_map[x].size();y++){

                visualization_msgs::Marker marker;
                marker.header.frame_id = global_frame_;
                marker.header.stamp = ros::Time::now();
                marker.id = ++i;
                marker.action = visualization_msgs::Marker::ADD;
                marker.type = visualization_msgs::Marker::CUBE;
                
                
                geometry_msgs::Point p;
                double wx,wy;
                moyingmap_->getCostMap()->mapToWorld(x+min_cx,y+min_cy,wx,wy);
                p.x = wx;
                p.y = wy;
                marker.pose.position.x = p.x;
                marker.pose.position.y = p.y;
                marker.pose.position.z = 0.05;
                marker.pose.orientation.w = 1;
                marker.scale.x = 0.45;
                marker.scale.y = 0.45;
                marker.scale.z = 0.05;
                std_msgs::ColorRGBA c;
                c.r = c.g = c.b = 0;

                int cost = local_cost_map[x][y];
                if(cost == 255)
                    c.a = 1;
                else if(cost == 0)
                    c.a = 0;
                else{
                    c.a = (cost*50)/255.0f;
                }
                marker.color = c;
                marker_array.markers.push_back(marker);
        
            }
        }
        local_cost_map_pub_.publish(marker_array);

        // visualization_msgs::Marker marker;
        // {
        //     marker.header.frame_id = "map";
        //     marker.header.stamp = ros::Time::now();
        //     marker.id = 0;
        //     marker.action = visualization_msgs::Marker::ADD;
        //     marker.pose.orientation.w = 1;
        //     marker.scale.x = 0.45;
        //     marker.scale.y = 0.45;
        //     marker.scale.z = 0.05;
        //     marker.color.a = 1;
        //     marker.type = visualization_msgs::Marker::CUBE_LIST;

        //     // for(int i = 0; i < local_obstacle.data().size(); i++)

        //     for(int x=0;x<local_cost_map.size();x++){
        //         for(int y=0;y<local_cost_map[x].size();y++){
        //             int cost = local_cost_map[x][y];
        //             geometry_msgs::Point p;
        //             double wx,wy;
        //             moyingmap_->getCostMap()->mapToWorld(x+min_cx,y+min_cy,wx,wy);
        //             p.x = wx;
        //             p.y = wy;
                    
        //             // moyingmap_->getCostMap()->mapToWorld(x, y, p.x, p.y);
        //             std_msgs::ColorRGBA c;
        //             c.r = 0;
        //             c.g = 0;
        //             c.b = 0;

        //             if(cost == 255)
        //                 c.a = 1;
        //             else if(cost == 0)
        //                 c.a = 0;
        //             else{
        //                 c.a = (cost*30)/255.0f;
        //             }

        //             marker.points.push_back(p);
        //             marker.colors.push_back(c);
            
        //         }
        //     }

        // }
        // // printf("======local cost map marker size:%d\n",marker.points.size());
        // local_cost_map_pub_.publish(marker);

    }


    void VehicleViz::publishCollidePolygon(const std::vector<navigation::Coordinate>& coor_vec){

        geometry_msgs::PolygonStamped polygon;
        polygon.header.frame_id = global_frame_;
        polygon.header.stamp = ros::Time::now();
        for(auto coor : coor_vec)
        {
            geometry_msgs::Point32 p;
            p.x = coor.x;
            p.y = coor.y;
            polygon.polygon.points.push_back(p);
        }

        collide_rect_pub_.publish(polygon);

    }

} // namespace name
}